/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlClient.hpp,v 1.13 2009-12-08 10:48:28 morsko Exp $
 *
 * \file
 * \brief MaCI - JointGroupCtrl Interface Client header
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 */
#ifndef _MACI_INTERFACE_JOINTGROUPCTRLCLIENT_HPP_
#define _MACI_INTERFACE_JOINTGROUPCTRLCLIENT_HPP_

#include "MaCI.hpp"
#include "thread.hpp"
#include "sync.hpp"
#include "JointGroupCtrlData.hpp"
// Forward declare gimi::GIMI
namespace gimi {
  class GIMI;
}

namespace MaCI {
  namespace JointGroupCtrl {
    /**
     * Struct for defining information about one joint.
     * \see EJointControlType
     */
    struct TJointInfo{
      TJointInfo(std::string aJointName, 
		 unsigned int aJointNumber,
		 EJointControlType aControlType,
		 bool aCanControlSpeed = false,
		 float aMaxSpeed = 0.0,
		 bool aCanControlPosition = false,
		 float aMinPosition = 0.0,
		 float aMaxPosition = 0.0,
                 bool aCanControlForce = false,
                 float aMinForce = 0.0,
                 float aMaxForce = 0.0,
                 TDenavitHartenberParameter aDHParameter = TDenavitHartenberParameter()):
	jointName(aJointName), 
	jointNumber(aJointNumber),
	jointControlType(aControlType),
	canControlSpeed(aCanControlSpeed), 
	maxSpeed(aMaxSpeed),
	canControlPosition(aCanControlPosition),
	minPosition(aMinPosition),
	maxPosition(aMaxPosition),
        canControlForce(aCanControlForce),
        minForce(aMinForce),
        maxForce(aMaxForce),
        DHParameter(aDHParameter){}
      
      std::string jointName;             ///<Name of the joint
      unsigned int jointNumber;          ///<Index of the joint
      MaCI::JointGroupCtrl::EJointControlType jointControlType;///<Control type of the joint(angular or linear)
      bool canControlSpeed;              ///<Tells if the joint can be commanded with speed command
      float maxSpeed;                    ///<Maximum speed, so the setSpeed can be commanded -max...max
      bool canControlPosition;           ///<Tells if the joint can be commanded with position command
      float minPosition;                 ///<Minimum position value of the set position-command
      float maxPosition;                 ///<Maximum position value of the set position-command
      bool canControlForce;              ///<Tells if the joint can be commanded with force commands.
      float minForce;                    ///<Minimum force value for joint controlling
      float maxForce;                    ///<Maximum force value for joint controlling
      MaCI::JointGroupCtrl::TDenavitHartenberParameter DHParameter; ///< Denavit hartenbegt parameter
    };

    typedef std::vector<TJointInfo> TJointGroupInformation;
   
    typedef std::vector<TJointSpeed> TJointSpeedArray;  
    typedef std::vector<TJointPosition> TJointPositionArray;  
    typedef std::vector<TJointForce> TJointForceArray;
    /** MaCI - JointGroupCtrl Client interface.
     * 
     * This class contains the methods and types for using
     * a MaCI::JointGroupCtrl service in the GIMnet.
     */
    class CJointGroupCtrlClient : private gim::CSync,
				  private gim::CThread,
				  public MaCI::CMaCI
    {
    public:
      /** Constructor.
       * 
       * Constructor of the JointGourpCtrlClient. This requires;
       * a pointer to Initialized and connected GIMI, minor
       * number of this interface (Used as GIMI minor number)
       *
       * @param[in] aGIMIPtr Pointer to GIMI used for communication
       * @param[in] aInterfaceMinor Interface minor number. Usually
       *            used to indicate the instance of devices of same
       *            type.
       */     
      CJointGroupCtrlClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor = -1);

      
      /** Destructor.
       */
      ~CJointGroupCtrlClient();
      

      /** Open client interface.
       */
      EMaCIError DoOpen(void);


      /** Close client interface.
       */
      EMaCIError DoClose(void);

      /** Set speed.
       *
       * This function sends a command attempting to set a speed to a joint
       *
       * NOTE: All joints can't be commanded by giving speed commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
       * 
       * @param[in] aSpeed           Speed in [rad/s] or [m/s] depends of the control type
       * @param[in] aNumberOfJoint   The number of joint to control
       *
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no connection, etc.)
       */
      bool SetSpeed(const float aSpeed, const int aNumberOfJoint);
      

      /** Set Group Speed
       *
       * This function set speeds to multiple joint at the same time.
       *
       * NOTE: All joints can't be commanded by giving speed commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
       *
       * @param[in] aJointSpeedArray  An array of TJointSpeeds. This describes the
       *                              joints to move
       * @return                      'true' if the command was succesfully
       *                              _sent_. Note that the return result 'true' does not
       *                              necessarily mean that the command is
       *                              really received and executed by the server.
       *                              'false' in case the send failed for some reason.
       *                              (No such client, no connection, etc.)
       */
      bool SetGroupSpeed(MaCI::JointGroupCtrl::TJointSpeedArray &INPUT);
    
      /** Set position.
       *
       * This function sends a command attempting to set a joint to a to the wanted
       * position
       *
       * NOTE: All joints can't be commanded by giving position commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
 
       * @param[in] aPosition        Position of the joint in [rad] or [m] depends of the control type.
       * @param[in] aNumberOfJoint   The number of joint to control
       *
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no connection, etc.)
       */
      bool SetPosition(const float aPosition, const int aNumberOfJoint);

      /** Set Group Position
       *
       * This function set multiple joint to the wanted position at the same time.
       *
       * NOTE: All joints can't be commanded by giving position commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
       *
       * @param[in] aJointPositionArray  An array of TJointPositions. This describes the
       *                                 joints to move
       * @return                         'true' if the command was succesfully
       *                                 _sent_. Note that the return result 'true' does not
       *                                 necessarily mean that the command is
       *                                 really received and executed by the server.
       *                                 'false' in case the send failed for some reason.
       *                                 (No such client, no connection, etc.)
       */
      bool SetGroupPosition(MaCI::JointGroupCtrl::TJointPositionArray &INPUT);

      /** Set force
       *
       * This function sends a command attempting to set a force to the wanted
       * joint 
       * 
       * NOTE: All joints can't be commanded by giving force commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
       *
       * @param[in] aPosition        Force of the joint in [N] depends of the control type.
       * @param[in] aNumberOfJoint   The number of joint to control
       *
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no con nection, etc.)
       */
      bool SetForce(const float aForce, const int aNumberOfJoint);

  
      /** Set Group Force
       *
       * This function set force values to multiple joints at the same time.
       *
       * NOTE: All joints can't be commanded by giving force commands. Check with
       *       GetJointGroupInformation-function how the joints can be commanded.
       *
       * @param[in] aJointForceArray  An array of TJointForces. This describes the
       *                              joints to move
       * @return                      'true' if the command was succesfully
       *                              _sent_. Note that the return result 'true' does not
       *                              necessarily mean that t he command is
       *                              really received and executed by the server.
       *                              'false' in case the send failed for some reason.
       *                              (No such client, no connection, etc.)
       */
      bool SetGroupForce(MaCI::JointGroupCtrl::TJointForceArray &INPUT);

      /**
       * Returns the speed of wanted joint.
       *
       * @param[in] aJointNumber The number of the joint
       * @param[out] aSpeed      Speed of the joint is stored here. [rad/s] or [m/s] depends of the control type.
       * @param[in] aTimeout_ms  Timeout value. If the operation is not completed
       *                         before timeout is reached, the operation fails.
       * @return                 'true' if the command was succesfully
       *                         _sent_. Note that the return result 'true' does not
       *                         necessarily mean that the command is
       *                         really received and executed by the server.
       *                         'false' in case the send failed for some reason.
       *                         (No such client, no connection, etc.)
       */
      bool GetSpeed(const int aJointNumber,float &aSpeed,unsigned int aTimeout_ms);

      /**
       * Returns the speeds of the whole group
       *
       * @param[out] OUTPUT  The speed values are stored int this array
       * @param[in][out] INOUT          Pointer to sequence number. If this is NULL,
       *                               the most recent scan available is returned immediately.
       *                               If this is not NULL, the value pointed
       *                               should be set to 0 on the first call to this function.
       *                               After each call, the sequence number of the returned
       *                               scan is stored here. If user wants to read all the
       *                               measures available, this value should not be
       *                               modified after the first call.
       * @param[in] aTimeout_ms        Timeout value. If the operation is not completed before
       *                               timeout is reached, the operation fails.
       * @return                       True if operation was succesfull and new data is stored
       *                               in the array, otherwise false
       */
      bool GetGroupSpeeds(MaCI::JointGroupCtrl::TJointSpeedArray &OUTPUT,
                         unsigned int *INOUT,
                         unsigned int aTimeout_ms);


     /**
       * Returns the position of wanted joint.
       *
       * @param[in] aJointNumber    The number of the joint
       * @param[out] aPosition      Position of the joint is stored here. [rad] or [m] depends of the control type.
       * @param[in] aTimeout_ms     Timeout value. If the operation is not completed
       *                            before timeout is reached, the operation fails.
       * @return                    'true' if the command was succesfully
       *                            _sent_. Note that the return result 'true' does not
       *                            necessarily mean that the command is
       *                            really received and executed by the server.
       *                            'false' in case the send failed for some reason.
       *                            (No such client, no connection, etc.)
       */
      bool GetPosition(const int aJointNumber, float &aPosition,unsigned int aTimeout_ms);

      /**
       * Returns the positions of the whole group
       *
       * @param[out] OUTPUT  The position values are stored int this array
       * @param[in][out] INOUT             Pointer to sequence number. If this is NULL,
       *                                  the most recent scan available is returned immediately.
       *                                  If this is not NULL, the value pointed
       *                                  should be set to 0 on the first call to this function.
       *                                  After each call, the sequence number of the returned
       *                                  scan is stored here. If user wants to read all the
       *                                  measures available, this value should not be
       *                                  modified after the first call.
       * @param[in] aTimeout_ms           Timeout value. If the operation is not completed before
       *                                  timeout is reached, the operation fails.
       * @return                          True if operation was succesfull and new data is stored
       *                                  in the array, otherwise false
       */
      bool GetGroupPositions(MaCI::JointGroupCtrl::TJointPositionArray &OUTPUT,
                            unsigned int *INOUT,
                            unsigned int aTimeout_ms);

     /**
       * Returns the force of wanted joint.
       *
       * @param[in] aJointNumber    The number of the joint
       * @param[out] aForce         Force of the joint is stored here in [N]
       * @param[in] aTimeout_ms     Timeout value. If the operation is not completed
       *                            before timeout is reached, the operation fails.
       * @return                    'true' if the command was succesfully
       *                            _sent_. Note that the return result 'true' does not
       *                            necessarily mean that the command is
       *                            really received and executed by the server.
       *                            'false' in case the send failed for some reason.
       *                            (No such client, no connection, etc.)
       */
      bool GetForce(const int aJointNumber, float &aForce,unsigned int aTimeout_ms);

      /**
       * Returns the forces of the whole group
       *
       * @param[out] OUTPUT  The force values are stored int this array
       * @param[in][out] INOUT          Pointer to sequence number. If this is NULL,
       *                               the most recent scan available is returned immediately.
       *                               If this is not NULL, the value pointed
       *                               should be set to 0 on the first call to this function.
       *                               After each call, the sequence number of the returned
       *                               scan is stored here. If user wants to read all the
       *                               measures available, this value should not be
       *                               modified after the first call.
       * @param[in] aTimeout_ms        Timeout value. If the operation is not completed before
       *                               timeout is reached, the operation fails.
       * @return                       True if operation was succesfull and new data is stored
       *                               in the array, otherwise false
       */
      bool GetGroupForces(MaCI::JointGroupCtrl::TJointForceArray &OUTPUT,
                         unsigned int *INOUT,
                         unsigned int aTimeout_ms);
    

      /**
       * Return the joint information array which describes all the joints of
       * the group. One TJointInfo contains information about one joint, name of the joint, 
       * number of the joint, information if the joint can be control with speed information 
       * and information if the joint can be control with position information
       *
       * @param[out] aInfo      The joint information array
       * @param[in] aTimeout_ms Timeout value. If the operation is not completed
       *                        before timeout is reached, the operation fails.
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no connection, etc.)
       */
      bool GetJointGroupInformation(MaCI::JointGroupCtrl::TJointGroupInformation &OUTPUT, 
				    unsigned int aTimeout_ms);

      /**
       * Return number of joints in the group. This helper function utilizing .
       * GetJointGroupInformation() method.
       *
       * 
       * @return                Number of joints, 0 if no info available. 
       */
      int GetJointNumber(void);

      /**
       * Get information about specific joint.
       * 
       *
       * @param[out] OUTPUT      The joint information struct
       * @param[in] aNumber     Number of the joint
       * @param[in] aTimeout_ms Timeout value. If the operation is not completed
       *                        before timeout is reached, the operation fails.
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no connection, etc.)
       */
      bool GetJointInformation(MaCI::JointGroupCtrl::TJointInfo &OUTPUT, int aNumber,
				    unsigned int aTimeout_ms);
	

      /*
       * Return the joint group information array which describes the state of
       * joint group server(initializing, open,closed...)
       *
       * @param[out] aStatus    The joint group status 
       * @param[in] aTimeout_ms Timeout value. If the operation is not completed
       *                        before timeout is reached, the operation fails.
       * @return                'true' if the command was succesfully
       *                        _sent_. Note that the return result 'true' does not
       *                        necessarily mean that the command is
       *                        really received and executed by the server.
       *                        'false' in case the send failed for some reason.
       *                        (No such client, no connection, etc.)
       */
      bool GetJointGroupStatus(MaCI::JointGroupCtrl::EJointGroupStatus &OUTPUT,
                                      unsigned int aTimeout_ms);

private:
      /// Thread runner function. Is in charge of receiving events.
      int ThreadFunction(const int aThreadNumber);
      int ProcessMessage(const gimi::GIMIMessage &msg);
      bool GetEventTypes();
      bool RequestStateOfServer();
      std::map<int,CJointGroupCtrlData *> iReplyMessages;
      int iCmdSequence;

      //Event data saved to these variables...
      CJointGroupCtrlData iSpeedData;     ///< newest speed data event is stored here
      unsigned int iSpeedDataSequence;    ///< sequence number of speed data
      CJointGroupCtrlData iPositionData;  ///< newest position data event is stored here
      unsigned int iPositionDataSequence; ///< sequence number of position data
      CJointGroupCtrlData iForceData;     ///< newest force data event is stored here
      unsigned int iForceDataSequence;    ///< sequence number of force data

      enum EJointGetters { 
        KReplyJointInfo = 0,        ///< Joint info
	KReplySpeed = 1,            ///< Get speed reply
	KReplyPosition = 2,         ///< Get Position reply
        KReplyJointGroupStatus = 3, ///<Get Joint group status
        KReplyForce = 4,            ///<Get force reply
        KGroupSpeeds = 5,
        KGroupPositions = 6,
        KGroupForces = 7
      };

      TEventTypes iEventTypes;
    };

  }
}

#endif //_MACI_INTERFACE_JOINTGROUPCTRLCLIENT_HPP_
